#include "../include/wheel_tf.hpp"
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>

wheel_tf_class::wheel_tf_class(ros::NodeHandle* nodehand):nh_(*nodehand)
{
    my_timer_ = nh_.createTimer(ros::Duration(1.0),boost::bind(&wheel_tf_class::wheel_tf_pub,this));   
}

wheel_tf_class::~wheel_tf_class()
{

}

void wheel_tf_class::wheel_tf_pub()
{
    ROS_WARN("wheel_tf_pub"); 
    geometry_msgs::TransformStamped tf_right_wheel;

    tf_right_wheel.header.stamp = ros::Time::now();
    tf_right_wheel.header.frame_id = "base_link";
    tf_right_wheel.child_frame_id = "right_wheel";

    tf_right_wheel.transform.translation.x = 0.3;
    tf_right_wheel.transform.translation.y = -0.45;
    tf_right_wheel.transform.translation.z = -0.2;

    tf2::Quaternion qtn_right;
    qtn_right.setRPY(1.57,0,0);
    tf_right_wheel.transform.rotation.x = qtn_right.getX();
    tf_right_wheel.transform.rotation.y = qtn_right.getY();
    tf_right_wheel.transform.rotation.z = qtn_right.getZ();
    tf_right_wheel.transform.rotation.w = qtn_right.getW();

    tf_broadcaster_right_.sendTransform(tf_right_wheel);

    geometry_msgs::TransformStamped tf_left_wheel;
    tf_left_wheel.header.stamp = ros::Time::now();
    tf_left_wheel.header.frame_id = "base_link";
    tf_left_wheel.child_frame_id = "left_wheel";

    tf_left_wheel.transform.translation.x = 0.3;
    tf_left_wheel.transform.translation.y = 0.45;
    tf_left_wheel.transform.translation.z = -0.2;

    tf2::Quaternion qtn_left;
    qtn_left.setRPY(1.57,0,0);
    tf_left_wheel.transform.rotation.x = qtn_left.getX();
    tf_left_wheel.transform.rotation.y = qtn_left.getY();
    tf_left_wheel.transform.rotation.z = qtn_left.getZ();
    tf_left_wheel.transform.rotation.w = qtn_left.getW();

    tf_broadcaster_left_.sendTransform(tf_left_wheel);
}

int main(int argc, char **argv)
{
    ros::init(argc,argv,"wheel_tf_node");
    ros::NodeHandle nh;
    
    // wheel_tf_obj =new wheel_tf_class(nh);
    wheel_tf_class wheel_tf_obj(&nh);
    ros::spin();
    return 0;
}


